const std = @import("std");

pub const Vec2 = @import("vec2.zig");

pub const Point = @This();

pub const Origin = Point.init(0, 0);

x: f32,
y: f32,

pub fn init(x: f32, y: f32) Point {
    return Point{
        .x = x,
        .y = y,
    };
}

pub fn vectorTo(self: *const Point, other: *const Point) Vec2 {
    return Vec2.init(other.x - self.x, other.y - self.y);
}

pub fn distanceSquaredTo(self: *const Point, other: *const Point) f32 {
    const x = self.x - other.x;
    const y = self.y - other.y;

    return x * x + y * y;
}

pub fn move2Origin(self: *Point, x: f32, y: f32) void {
    self.x -= x;
    self.y -= y;
}

pub fn ifMove2Origin(self: *const Point, x: f32, y: f32) Point {
    return Point{
        .x = self.x - x,
        .y = self.y - y,
    };
}

pub fn moveAwayOrigin(self: *Point, x: f32, y: f32) void {
    self.x += x;
    self.y += y;
}

pub fn ifMoveAwayOrigin(self: *const Point, x: f32, y: f32) Point {
    return Point{
        .x = self.x + x,
        .y = self.y + y,
    };
}

pub fn move(self: *Point, vector: *const Vec2) void {
    self.x += vector.x;
    self.y += vector.y;
}

pub fn ifMove(self: *const Point, vector: *const Vec2) Point {
    return Point{
        .x = self.x + vector.x,
        .y = self.y + vector.y,
    };
}

pub fn newMinPoint(self: *const Point, other: *const Point) Point {
    return Point{
        .x = @min(self.x, other.x),
        .y = @min(self.y, other.y),
    };
}

pub fn newMaxPoint(self: *const Point, other: *const Point) Point {
    return Point{
        .x = @max(self.x, other.x),
        .y = @max(self.y, other.y),
    };
}

pub fn transform(self: *Point, tx: *const Transform) void {
    const x: f32 = (tx.rotation.cosine * self.x - tx.rotation.sine * self.y) + tx.point.x;
    const y: f32 = (tx.rotation.sine * self.x + tx.rotation.cosine * self.y) + tx.point.y;

    self.x = x;
    self.y = y;
}

pub fn ifTransform(self: *const Point, tx: *const Transform) Point {
    return Point{
        .x = (tx.rotation.cosine * self.x - tx.rotation.sine * self.y) + tx.point.x,
        .y = (tx.rotation.sine * self.x + tx.rotation.cosine * self.y) + tx.point.y,
    };
}

/// 某点从当前本地坐标系转换至另一个本地坐标系
pub fn transform2OtherLocal(self: *Point, selfTx: *const Transform, otherTx: *const Transform) void {
    const w_x: f32 = (selfTx.rotation.cosine * self.x - selfTx.rotation.sine * self.y) + selfTx.point.x;
    const w_y: f32 = (selfTx.rotation.sine * self.x + selfTx.rotation.cosine * self.y) + selfTx.point.y;

    const o_l_x: f32 = w_x - otherTx.point.x;
    const o_l_y: f32 = w_y - otherTx.point.y;

    self.x = otherTx.rotation.cosine * o_l_x + otherTx.rotation.sine * o_l_y;
    self.y = -otherTx.rotation.sine * o_l_x + otherTx.rotation.cosine * o_l_y;
}

pub fn reverse(self: *Point, t: *const Transform) void {
    const vx: f32 = self.x - t.point.x;
    const vy: f32 = self.y - t.point.y;

    self.x = t.rotation.cosine * vx + t.rotation.sine * vy;
    self.y = -t.rotation.sine * vx + t.rotation.cosine * vy;
}

pub fn ifReverse(self: *const Point, t: *const Transform) Point {
    const vx: f32 = self.x - t.point.x;
    const vy: f32 = self.y - t.point.y;

    return Point{
        .x = t.rotation.cosine * vx + t.rotation.sine * vy,
        .y = -t.rotation.sine * vx + t.rotation.cosine * vy,
    };
}

pub fn rotate(self: *Point, rotation: *const Rotation) void {
    const x = rotation.cosine * self.x - rotation.sine * self.y;
    const y = rotation.sine * self.x + rotation.cosine * self.y;

    self.x = x;
    self.y = y;
}

pub fn ifRotate(self: *const Point, rotation: *const Rotation) Point {
    return Point{
        .x = rotation.cosine * self.x - rotation.sine * self.y,
        .y = rotation.sine * self.x + rotation.cosine * self.y,
    };
}

pub fn inverseRotate(self: *Point, rotation: *const Rotation) void {
    const x = rotation.cosine * self.x + rotation.sine * self.y;
    const y = -rotation.sine * self.x + rotation.cosine * self.y;

    self.x = x;
    self.y = y;
}

pub fn ifInverseRotate(self: *const Point, rotation: *const Rotation) Point {
    return Point{
        .x = rotation.cosine * self.x + rotation.sine * self.y,
        .y = -rotation.sine * self.x + rotation.cosine * self.y,
    };
}

pub fn neg(self: *Point) void {
    self.x = -self.x;
    self.y = -self.y;
}

pub fn ifNeg(self: *const Point) Point {
    return Point{
        .x = -self.x,
        .y = -self.y,
    };
}

/// A 2D rigid transform
pub const Transform = struct {
    point: Point,
    rotation: Rotation,

    pub fn init(point: Point, radians: f32) Transform {
        return Transform{
            .point = point,
            .rotation = Rotation.init(radians),
        };
    }

    /// Multiply two transforms. If the result is applied to a point p local to frame B,
    /// the transform would first convert p to a point local to frame A, then into a point
    /// in the world frame.
    /// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
    ///    = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
    pub fn mul(self: *Transform, other: *const Transform) void {
        const _point = other.point.ifRotate(self.rotation);
        self.point.moveAwayOrigin(_point.x, _point.y);
        self.rotation.rotate(other.rotation);
    }

    /// Creates a transform that converts a local point in frame self to a local point in frame other.
    /// v2 = other.q' * (self.q * v1 + self.p - other.p)
    ///    = other.q' * self.q * v1 + other.q' * (self.p - other.p)
    pub fn inverseMul(self: *Transform, other: *const Transform) void {
        const _point = self.point.ifMove2Origin(other.x, other.y);
        self.point = _point.ifInverseRotate(other.rotation);
        self.rotation.inverseRotate(other.rotation);
    }

    pub fn ifInverseMul(self: *const Transform, other: *const Transform) Transform {
        const _point = self.point.ifMove2Origin(other.point.x, other.point.y);

        return Transform{
            .point = _point.ifInverseRotate(&other.rotation),
            .rotation = self.rotation.ifInverseRotate(&other.rotation),
        };
    }
};

/// 2D rotation
/// This is similar to using a complex number for rotation
pub const Rotation = struct {
    cosine: f32,
    sine: f32,

    pub fn init(radians: f32) Rotation {
        return Rotation{
            .cosine = @cos(radians),
            .sine = @sin(radians),
        };
    }

    pub fn rotate(self: *Rotation, other: *const Rotation) void {
        const c = self.cosine * other.cosine - self.sine * other.cosine;
        const s = self.sine * other.cosine + self.cosine * other.sine;

        self.cosine = c;
        self.sine = s;
    }

    pub fn ifRotate(self: *const Rotation, other: *const Rotation) Rotation {
        return Rotation{
            .cosine = self.cosine * other.cosine - self.sine * other.cosine,
            .sine = self.sine * other.cosine + self.cosine * other.sine,
        };
    }

    pub fn inverseRotate(self: *Rotation, other: *const Rotation) void {
        const s = other.cosine * self.sine - other.sine * self.cosine;
        const c = other.cosine * self.cosine + other.sine * self.sine;

        self.sine = s;
        self.cosine = c;
    }

    pub fn ifInverseRotate(self: *const Rotation, other: *const Rotation) Rotation {
        return Rotation{
            .sine = other.cosine * self.sine - other.sine * self.cosine,
            .cosine = other.cosine * self.cosine + other.sine * self.sine,
        };
    }
};
